
// import oimo.dynamics.TimeStep;
// import oimo.m.Mat3;
// import oimo.m.Vec3;
// import oimo.m.M;
// import oimo.dynamics.*;
// import oimo.dynamics.constraint.info.JacobianRow;
// import oimo.dynamics.constraint.info.joint.JointSolverInfo;
// import oimo.dynamics.constraint.info.joint.JointSolverInfoRow;
// import oimo.dynamics.constraint.joint.base.*;

import { Joint } from "./Joint";
import { SpringDamper } from "./SpringDamper";
import { SphericalJointConfig } from "./SphericalJointConfig";
import { JointType } from "./JointType";
import { JointSolverInfo } from "../info/joint/JointSolverInfo";
import { TimeStep } from "../../TimeStep";
import { Vec3 } from "../../../common/Vec3";
import { M } from "../../../common/M";
import { JointMacro } from "./JointMacro";
import { Mat3 } from "../../../common/Mat3";
import { JointSolverInfoRow } from "../info/joint/JointSolverInfoRow";
import { JacobianRow } from "../info/JacobianRow";

/**
 * A spherical joint (a.k.a. ball and socket joint) constrains two rigid bodies to share
 * their anchor points. This joint provides three degrees of freedom. You can enable a
 * spring and damper effect of the constraint.
 */

export class SphericalJoint extends Joint {
	_sd: SpringDamper;

	/**
	 * Creates a new spherical joint by configuration `config`.
	 */
	constructor(config: SphericalJointConfig) {
		super(config, JointType._SPHERICAL);

		this._sd = config.springDamper.clone();
	}

	// --- ---

	public getInfo(info: JointSolverInfo, timeStep: TimeStep, isPositionPart: boolean): void {
		if (this._sd.frequency > 0 && isPositionPart) return;

		// compute positional error
		var error: Vec3 = new Vec3();
		M.vec3_sub(error, this._anchor2, this._anchor1);

		// compute CFM and ERP
		var cfm: number;
		var erp: number;
		if (this._sd.frequency > 0) {
			let result = JointMacro.computeSoftConstraintParameters(this._sd.frequency, this._sd.dampingRatio, timeStep.dt,
				this._sd.useSymplecticEuler);
			cfm = result[0], erp = result[1];
			cfm *= this._b1._invMass + this._b2._invMass;
		} else {
			cfm = 0;
			erp = this.getErp(timeStep, isPositionPart);
		}

		// compute rhs
		var linearRhs: Vec3 = new Vec3();
		M.vec3_scale(linearRhs, error, erp);
		var linRhsX: number = M.vec3_get(linearRhs, 0);
		var linRhsY: number = M.vec3_get(linearRhs, 1);
		var linRhsZ: number = M.vec3_get(linearRhs, 2);

		var crossR1: Mat3 = new Mat3();
		var crossR2: Mat3 = new Mat3();
		M.vec3_toCrossMatrix(crossR1, this._relativeAnchor1);
		M.vec3_toCrossMatrix(crossR2, this._relativeAnchor2);
		M.mat3_negate(crossR1, crossR1);
		M.mat3_negate(crossR2, crossR2);

		var row: JointSolverInfoRow;
		var j: JacobianRow;

		// linear X
		row = info.addRow(this._impulses[0]);
		row.equalLimit(linRhsX, cfm);

		j = row.jacobian;
		M.vec3_set(j.lin1, 1, 0, 0);
		M.vec3_set(j.lin2, 1, 0, 0);
		M.mat3_getRow(j.ang1, crossR1, 0);
		M.mat3_getRow(j.ang2, crossR2, 0);

		// linear Y
		row = info.addRow(this._impulses[1]);
		row.equalLimit(linRhsY, cfm);

		j = row.jacobian;
		M.vec3_set(j.lin1, 0, 1, 0);
		M.vec3_set(j.lin2, 0, 1, 0);
		M.mat3_getRow(j.ang1, crossR1, 1);
		M.mat3_getRow(j.ang2, crossR2, 1);

		// linear Z
		row = info.addRow(this._impulses[2]);
		row.equalLimit(linRhsZ, cfm);

		j = row.jacobian;
		M.vec3_set(j.lin1, 0, 0, 1);
		M.vec3_set(j.lin2, 0, 0, 1);
		M.mat3_getRow(j.ang1, crossR1, 2);
		M.mat3_getRow(j.ang2, crossR2, 2);
	}

	// --- internal ---

	public _getVelocitySolverInfo(timeStep: TimeStep, info: JointSolverInfo): void {
		super._getVelocitySolverInfo(timeStep, info);
		this.getInfo(info, timeStep, false);
	}

	public _getPositionSolverInfo(info: JointSolverInfo): void {
		super._getPositionSolverInfo(info);
		this.getInfo(info, null, true);
	}

	// --- public ---

	/**
	 * Returns the spring and damper settings.
	 */
	public getSpringDamper(): SpringDamper {
		return this._sd;
	}
}
